%% 倾斜轨道同步卫星(北斗三号IGSO-3)
close all;  clear;  clc;

global precision
precision = 0.0002;  % 卫星位置精度

writer = VideoWriter('demo3.mp4','MPEG-4');
open(writer);

%% 绘制卫星轨道
figure('position', get(0,'ScreenSize'));
tiledlayout(6,3,'TileSpacing','none','Padding','compact');
ax1 = nexttile([5,1]);
hold on
grid on
title("卫星绕行地球过程",'FontSize',12,'FontWeight','bold');
G = 6.754e-11;
M_earth = 5.965e24;
hStltMin = 35700      % km
hStltMax = 35869      % km
kT = 1.00278522;
a = power(G * M_earth * (kT*24*3600)^2 / (4*pi^2), 1/3) / 6371000;
e = 0.0020067;
orbit_i = 57.6423/180*pi;
Omega = 298.8356/180*pi;
omega = 186.7710/180*pi;
theta0 = -8.5306/180*pi+pi;
[xt, yt, zt] = drawOrbit(a, e, orbit_i, Omega, omega);

len = length(xt);
idx = 1;

hAn1 = annotation('textbox',[.04, .18, 1, .04],'String',['a=',num2str(a,'%.2f'),'（半长轴）'],'LineStyle','none');
hAn2 = annotation('textbox',[.04, .14, 1, .04],'String',['e=',num2str(e,'%.2f'),'（偏心率）'],'LineStyle','none');
hAn3 = annotation('textbox',[.15, .18, 1, .04],'String',['i=',num2str(orbit_i/pi*180,'%.2f'),'\circ（轨道倾角）'],'LineStyle','none');
hAn4 = annotation('textbox',[.15, .14, 1, .04],'String',['\Omega=',num2str(Omega/pi*180,'%.2f'),'\circ（升交点赤经）'],'LineStyle','none');
hAn5 = annotation('textbox',[.28, .18, 1, .04],'String',['\omega=',num2str(omega/pi*180,'%.2f'),'\circ（近地点张角）'],'LineStyle','none');
hAn6 = annotation('textbox',[.28, .14, 1, .04],'String',['f=',num2str(idx/len*360,'%.2f'),'\circ（真近点角）'],'LineStyle','none');

rho = zeros(1,len);
for i = 1:len
    rho(i) = norm([xt(i),yt(i),zt(i)]);
end

%% 绘制卫星及星下点
hSatellite = scatter3(xt(idx),yt(idx),zt(idx),10,'r','fill');    % 卫星
hSubastral = scatter3(xt(1)/rho(1),yt(1)/rho(1),zt(1)/rho(1),4,'fill');   % 星下点
hL3 = plot3([0,xt(idx)],[0,yt(idx)],[0,zt(idx)],'y','LineWidth',0.01);

%% 绘制地球
gridNum = 24;   % longitude: 360/24=15 degree/grid    latitude: 180/24=7.5 degree/grid
[x, y, z] = sphere(gridNum);
earth = imread('landOcean.jpg');

% 高配
clouds = imread('cloudCombined.jpg');

hEarth = surface(x ,y, z);
hEarth.FaceColor = 'texturemap';        % set color to texture mapping
hEarth.EdgeColor = 'none';              % remove surface edge color
% hEarth.EdgeAlpha = 0.3;
hEarth.CData = earth;                % set color data 

hCloud = surface(x*1.01,y*1.01,z*1.01); 
hCloud.FaceColor = 'texturemap';        % set color to texture mapping
hCloud.EdgeColor = 'none';              % remove surface edge color
hCloud.CData = clouds;                  % set color data 
 
hCloud.FaceAlpha = 'texturemap';        % set transparency to texture mapping
hCloud.AlphaData = max(clouds,[],3);    % set transparency data 

view([-150 10])                           % specify viewpoint 
axis equal   

%% 开始运动
nEarth = 1;   % 仿真时间内地球所转圈数
step = 1000;
dThetaEarth = 2*pi/step;  % 地球转速（每帧）
% dThetaEarth = 0;
vdEarth = 2*pi/(24*3600);
kv = sqrt(G * M_earth * (1+e) / (a * 6371000 * (1-e))^3) / vdEarth;
dThetaStltMax =  kv * dThetaEarth;          % 卫星近地点转速（每帧）
C = dThetaStltMax / precision * rho(1)^2;   % dIdx * r^2 == C
numOfFrame = round(step*nEarth);            % 仿真总帧数

az = zeros(1,numOfFrame+1);
el = zeros(1,numOfFrame+1);
[az(1),el(1)] = cart2sph(xt(1)/rho(1),yt(1)/rho(1),zt(1)/rho(1));
az(1) = az(1) - theta0;
while az(1)<-pi
    az(1) = az(1)+2*pi;
end
while az(1)>pi
    az(1) = az(1)-2*pi;
end

ax2 = nexttile([5,2]);
hold on
title("星下点轨迹",'FontSize',12,'FontWeight','bold');
image([-180,180], [90,-90], earth, 'AlphaData', 0.8);
hL4 = plot(az(1)*180/pi, el(1)*180/pi, 'r', 'LineWidth', 1);
scatter(az(1)*180/pi, el(1)*180/pi, 6,  'r', 'filled');
axis tight
axis equal
set(ax2,'XTick',-180:30:180,'YTick',-90:30:90);
grid on
ax2.GridAlpha = 1;
slen = 0;
dslen = 30;

M = makehgtform('axisrotate',[0,0,-1],theta0);
m = M(1:3,1:3);

for i = 1:gridNum+1
    for j = 1:gridNum+1
        tmp = [x(i,j),y(i,j),z(i,j)]*m;
        x(i,j) = tmp(1);
        y(i,j) = tmp(2);
        z(i,j) = tmp(3);
    end
end
% refresh
set(hEarth,'XData',x,'YData',y,'ZData',flip(z));
set(hCloud,'XData',x,'YData',y,'ZData',flip(z));
    
for t = 1:numOfFrame
    % 变换矩阵
    M = makehgtform('axisrotate',[0,0,-1],dThetaEarth);
    m = M(1:3,1:3);

    for i = 1:gridNum+1
        for j = 1:gridNum+1
            tmp = [x(i,j),y(i,j),z(i,j)]*m;
            x(i,j) = tmp(1);
            y(i,j) = tmp(2);
            z(i,j) = tmp(3);
        end
    end
    % refresh
    set(hEarth,'XData',x,'YData',y,'ZData',flip(z));
    set(hCloud,'XData',x,'YData',y,'ZData',flip(z));
    
    dIdx = round(C/rho(idx)^2);
    idx = idx+dIdx;
    if idx > len
        idx = idx-len;
    end
    % refresh
    set(hSatellite,'XData',xt(idx),'YData',yt(idx),'ZData',zt(idx));
    set(hSubastral,'XData',xt(idx)/rho(idx),'YData',yt(idx)/rho(idx),'ZData',zt(idx)/rho(idx));
    set(hL3,'XData',[0,xt(idx)],'YData',[0,yt(idx)],'ZData',[0,zt(idx)]);
    set(hL4,'XData',az(1:t)*180/pi,'YData',el(1:t)*180/pi);
    set(hAn6,'String',['f=',num2str(idx/len*360,'%.2f'),'\circ（真近点角）']);
    
    [az(t+1),el(t+1)] = cart2sph(xt(idx)/rho(idx),yt(idx)/rho(idx),zt(idx)/rho(idx));
    az(t+1) = az(t+1) - t*dThetaEarth - theta0;
    while az(t+1)<-pi
        az(t+1) = az(t+1)+2*pi;
    end
    while az(t+1)>pi
        az(t+1) = az(t+1)-2*pi;
    end
    if abs(az(t+1)-az(t))>1 || abs(el(t+1)-el(t))>1
        el(t+1) = NaN;
    end
    if ~isnan(el(t+1)) && ~isnan(el(t))
        u = diff(az([t,t+1])*180/pi);
        v = diff(el([t,t+1])*180/pi);
        l = hypot(u,v);
        slen = slen+l;
        if slen > dslen
            px = az(t+1)*180/pi - u/l/4;
            py = el(t+1)*180/pi - v/l/4;
            if abs(px) < 180 && abs(py) < 90
                quiver(px, py, u/l/4, v/l/4, 'r', 'LineWidth',3, 'MaxHeadSize',100);
            end
            slen = 0;
        end
    end

%     pause(0.01);
    % drawnow
    writeVideo(writer,getframe(gcf));
end

close(writer);